/****************************************************************************
 *Copyright(C),2024, Ningbo Tuopu Group Co., Ltd.
 *FileName:   RtcHdlr_Mgr.c
 *Date:       2024-12-16 19:03:04
 *Author:     TianGui.Wang
 *Version:    0
 *Path:       
 *Description:
 ****************************************************************************/
/****************************************************************************/
/*                             Include area                                 */
/****************************************************************************/
/* PRQA S 0380++ # M3CM-2_2-C99:Dir-1.1-C99 Number of macro definitions exceeds 4095
 * Reason: RTE error doesn't need to be repaired.*/
#include "MotAsp/MotAsp_Mgr.h"
/* PRQA S 0380--*/
/****************************************************************************/
/*                             Definition area                              */
/****************************************************************************/

/****************************************************************************/
/*                             Typedef area                                 */
/****************************************************************************/

/****************************************************************************/
/*                             Local data at RAM                            */
/****************************************************************************/

/****************************************************************************/
/*                             Global data at RAM                           */
/****************************************************************************/
MotAsp_Mgr_ObjType MotAsp = {0};
/****************************************************************************/
/*                        Global data at RAM declare area                   */
/****************************************************************************/

/****************************************************************************/
/*                             Local data at ROM                            */
/****************************************************************************/

/****************************************************************************/
/*                             Global data at ROM                           */
/****************************************************************************/

/****************************************************************************/
/*                        Global data at ROM declare area                   */
/****************************************************************************/

/****************************************************************************/
/*                        Local function declare area                       */
/****************************************************************************/

/****************************************************************************/
/*                        Global function declare area                      */
/****************************************************************************/

/****************************************************************************/
/*                      Global function Implementation area                 */
/****************************************************************************/

/****************************************************************************/
/*                      Local function Implementation area                  */
/****************************************************************************/

/*
*Function :                   MotAsp_Mgr_Vtaul_Calc                                 
*Brief    :         The motor opens the loop to calculate the angular position information 
*Para     :         MotAsp_Type_VIRTUAL_CALC_STRUCT * Virt: 
                         typedef struct {
                         uint32 Min_To_Cycle;
                         sint16 Rpm_Step;
                         sint32 Max_Angle;
                         uint8  Poles;

                         sint16 Rpm;
                         uint16 Angle;
                         }MotAsp_Type_VIRTUAL_CALC_STRUCT;                     
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Vtaul_Calc(MotAsp_Type_VIRTUAL_CALC_STRUCT * Virt)
{
     uint16_T MotAsp_rpm_t = 0u;
     uint16_T MotAsp_vtualspd = 0u;

     uint16_T MotAsp_angle_sum = 0u;
     uint16_T MotAsp_mot_dir = MOTASP_TYPE_VIRTAUL_INIT; 
		  
     if(Virt->Rpm > 0)
     {
          MotAsp_mot_dir = MOTASP_TYPE_VIRTAUL_UP;  /*Positive direction*/
          MotAsp_rpm_t += (uint16_T)(Virt->Rpm);
     }
     else if(Virt->Rpm < 0)
     {
          MotAsp_mot_dir = MOTASP_TYPE_VIRTAUL_DOWN;  /*Opposite direction*/
          MotAsp_rpm_t = (uint16_T)(0 - Virt->Rpm);
     }
     else 
     {
          MotAsp_mot_dir = 0u;  
          MotAsp_rpm_t = 0u;
     }

     MotAsp_rpm_t = Virt->Rpm;

     if(MotAsp_rpm_t > (uint16_T)(Virt->Rpm_Step))
     {
          MotAsp_rpm_t  =  MotAsp_rpm_t;
     }
     else if(MotAsp_rpm_t > 0u)
     {
          MotAsp_rpm_t  =  (uint16_T)(Virt->Rpm_Step);  
     }
     else
     {
          MotAsp_rpm_t = 0u;   
     }

     MotAsp_vtualspd = (uint16_T)((MotAsp_rpm_t * (Virt->Poles) * (uint16_T)(Virt->Max_Angle))/(Virt->Min_To_Cycle));//1us周期

     switch(MotAsp_mot_dir)
     {
          case MOTASP_TYPE_VIRTAUL_INIT:
               MotAsp_vtualspd = 0u;
			Virt->Angle = 0u;
          break;

          case MOTASP_TYPE_VIRTAUL_UP:
               if(Virt->Angle == (uint16_T)(Virt->Max_Angle))
               {
                    MotAsp_angle_sum = 0u;
               }
               else
               {
				MotAsp_angle_sum = Virt->Angle;
                    MotAsp_angle_sum += MotAsp_vtualspd;   
               }

               Virt->Angle = MotAsp_angle_sum;

               if(Virt->Angle >= (uint16_T)(Virt->Max_Angle))
               {
                    Virt->Angle =  (uint16_T)(Virt->Max_Angle);
               }
          break;

          case MOTASP_TYPE_VIRTAUL_DOWN:
               if(Virt->Angle == 0u)
               {
                    MotAsp_angle_sum = (uint16_T)(Virt->Max_Angle);
               }
               else
               {
				MotAsp_angle_sum = Virt->Angle;
                    MotAsp_angle_sum -= MotAsp_vtualspd;   
               }

			Virt->Angle = MotAsp_angle_sum;

               if(Virt->Angle < 0u)
               {
                    Virt->Angle =  0u;
               }
          break;

          default:
			Virt->Angle = 0u;
          break;
     }    
}

/*
*Function :                   MotAsp_Mgr_Learn_E_Zero                                 
*Brief    :         The electrical angle of the motor is zero 
*Para     :         MotAsp_Type_LEARN_E_ANGLE_STRUCT *Learn_E: 
                         typedef struct {
                         uint8  En;
                         uint8  State;
                         uint16  Angle;
                         MotAsp_Type_Variable_State_U16 Result;
                         }MotAsp_Type_LEARN_E_ANGLE_STRUCT;                 
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Learn_E_Zero(MotAsp_Type_LEARN_E_ANGLE_STRUCT *Learn_E)
{
     if(MotAsp_Base_Valid == Learn_E->En)
     {
          Learn_E->En = MotAsp_Base_InValid;

          Learn_E->Result.Sta = MotAsp_Base_Valid;
          Learn_E->M_Result.Sta = MotAsp_Base_Valid;
          
          Learn_E->Result.Val = Learn_E->Angle;
          Learn_E->M_Result.Val = Learn_E->M_Angle;

          Learn_E->State = MotAsp_Base_Valid;
     }
}

/* 
*Function :                   MotAsp_Mgr_Learn_P_Zero                                 
*Brief    :         Electrical mechanical angle zero
*Para     :         MotAsp_Type_LEARN_P_ANGLE_STRUCT *Learn_P 
                         typedef struct {
                         uint8  En;
                         uint8  State;
                         sint32  Pos;
                         MotAsp_Type_Variable_State_S32 Result;
                         }MotAsp_Type_LEARN_P_ANGLE_STRUCT;                     
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Learn_P_Zero(MotAsp_Type_LEARN_P_ANGLE_STRUCT *Learn_P)
{ 

    if(MotAsp_Base_Valid == Learn_P->En)
     {
          Learn_P->Result.Sta = MotAsp_Base_Valid;
          
          Learn_P->Result.Val = Learn_P->Pos;
          
          Learn_P->State = MotAsp_Base_Valid;
     }
}


/*
*Function :                   MotAsp_Mgr_Spd_Simple                                 
*Brief    :         Speed sampling function, sampling time 100us 
*Para     :         MotAsp_Type_SPD_SIMPLE_STRUCT *Spd_Simp: 
                         typedef struct {
                         sint16 *Date;
                         uint16 CNT;

                         uint16 Cnt;
                         sint16  Spd;
                         }MotAsp_Type_SPD_SIMPLE_STRUCT;                     
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Spd_Simple(MotAsp_Type_SPD_SIMPLE_STRUCT *Spd_Simp)
{
    Spd_Simp->Date[Spd_Simp->Cnt] = Spd_Simp->Spd;

    Spd_Simp->Cnt++;

    if(Spd_Simp->Cnt >= Spd_Simp->CNT) 
    {
        Spd_Simp->Cnt = 0u; 
    }
}

/*
*Function :                   MotAsp_Mgr_Spd_Calc                                 
*Brief    :         The velocity calculation function takes 1 ms
*Para     :         MotAsp_Type_SPD_CALC_STRUCT *Spd_Calc: 
                         typedef struct {
                         uint16 Poles;
                         uint16 CNT;
                         sint16 *Date;
                         uint32 Min_To_Cycle;
                         sint32 Max_Angle;
                         uint16 molecule0;
                         uint16 numerator0;
                         uint16 molecule1;
                         uint16 numerator1;

                         sint16  Spd;
                         }MotAsp_Type_SPD_CALC_STRUCT;                      
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Spd_Calc(MotAsp_Type_SPD_CALC_STRUCT *Spd_Calc)
{
    int32_T spd_val = 0;
    int32_T spd_sum = 0;
    
    for(uint8_T i = 0u ; i < Spd_Calc->CNT ; i++)
    {
      spd_val = spd_val + Spd_Calc->Date[i];
    }

    spd_val = spd_val / (int32_T)(Spd_Calc->CNT);
    spd_val = spd_val * (int32_T)(Spd_Calc->CNT);

    spd_val = (spd_val * (int32_T)(Spd_Calc->molecule0)) / (int32_T)(Spd_Calc->numerator0) + (Spd_Calc->Spd_Delay0 * (int32_T)(Spd_Calc->molecule1)) / (int32_T)(Spd_Calc->numerator1);

    Spd_Calc->Spd_Delay0 = spd_val;

    spd_sum = Spd_Calc->Spd_Delay0;

    Spd_Calc->Spd = (spd_sum * (int32_T)(Spd_Calc->Min_To_Cycle)) / (Spd_Calc->Max_Angle * (int32_T)(Spd_Calc->Poles));
}

/*
*Function :                   MotAsp_Mgr_Pos_Calc                                 
*Brief    :         A function that calculates location information
*Para     :         MotAsp_Type_ABS_POS_CALC_STRUCT *Abs_Calc: 
                         typedef struct {
                         uint16 Poles;
                         uint32 Lead_Screw;
                         sint32 Max_Angle;

                         MotAsp_Type_ABS_POS_CALC_STATE State;
                         uint64 Pos;
                         uint64 Pos_Basic;
                         uint64 Last_Rst_Pos; 
                         uint64 P_Learn; 
                         uint64 Result;
                         }MotAsp_Type_ABS_POS_CALC_STRUCT;                      
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Pos_Calc(MotAsp_Type_ABS_POS_CALC_STRUCT *Abs_Calc)
{
    int64_T Pos_E = 0;
    switch(Abs_Calc->State)
    {
          case MotAsp_Type_ABS_POS_CALC_INIT:
               if(Abs_Calc->Pos != 0)
               {
                    Abs_Calc->Pos_Basic = Abs_Calc->Pos;
                    Abs_Calc->State = MotAsp_Type_ABS_POS_CALC_NORMAL;
               }
               else
               {

               }

               Pos_E = 0;
          break;

          case MotAsp_Type_ABS_POS_CALC_LEARN_P:
               Abs_Calc->Pos_Basic = Abs_Calc->Pos;
               Pos_E = 0;
               Abs_Calc->Last_Rst_Pos = Abs_Calc->P_Learn;
               Abs_Calc->State = MotAsp_Type_ABS_POS_CALC_NORMAL;
          break;

          case MotAsp_Type_ABS_POS_CALC_NORMAL:
               Pos_E = Abs_Calc->Pos - Abs_Calc->Pos_Basic + Abs_Calc->Last_Rst_Pos - Abs_Calc->P_Learn;
          break;

          default:

          break;
     }

     Abs_Calc->Result = (int32_T)((float)((Pos_E) * (int32_T)(Abs_Calc->Lead_Screw)) / ((float)(Abs_Calc->Max_Angle * (int32_T)Abs_Calc->Poles)));
}


/*
*Function :                   MotAsp_Mgr_In                                 
*Brief    :         Input functions 
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                     
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_In(MotAsp_Mgr_ObjType *MotAsp)
{
     // MotAsp->Pri.MPS.D_SiCo_i[MotAsp_Lib_SIN_0].SinCos_V = MotAsp->In.Pie.ADC_SinN;
     // MotAsp->Pri.MPS.D_SiCo_i[MotAsp_Lib_SIN_1].SinCos_V = MotAsp->In.Pie.ADC_SinP;
     // MotAsp->Pri.MPS.D_SiCo_i[MotAsp_Lib_COS_0].SinCos_V = MotAsp->In.Pie.ADC_CosN;
     // MotAsp->Pri.MPS.D_SiCo_i[MotAsp_Lib_COS_1].SinCos_V = MotAsp->In.Pie.ADC_CosP;


     // if((MotAsp_Base_STrig_HigLev == MotAsp->In.Ctrl.CalOffsetAngle_RiseTrig)&&(MotAsp_Base_CAStep_Agl_0 == MotAsp->In.Ctrl.CAStep))
     // {
     //      MotAsp->Pri.Learn_E.En = MotAsp_Base_Valid;
     // }
     // else
     // {
     //      MotAsp->Pri.Learn_E.En = MotAsp_Base_InValid;
     // }

     // if(MotAsp_Base_STrig_HigLev == MotAsp->In.Ctrl.CalOffsetPos_RiseTrig)
     // {
     //      MotAsp->Pri.Learn_P.En = MotAsp_Base_Valid;
     // }
     // else
     // {
     //      MotAsp->Pri.Learn_P.En = MotAsp_Base_InValid;
     // }

     // if(MotAsp_Base_SMode_OpenLoop == MotAsp->In.Ctrl.Mode)     
     // {
     //      MotAsp->Pri.Open.Rpm = MotAsp->In.Ctrl.VirSpdRpm;
     // }
     // else
     // {
     //      MotAsp->Pri.Open.Rpm =  0;
     // }


     // if(MotAsp_Base_Valid != MotAsp->Init_Sig)
     // {
     //      MotAsp->Init_Sig = MotAsp_Base_Valid;
     //      MotAsp->Pri.Learn_E.Result.Val              =              MotAsp->In.NvM.E.Val;
     //      MotAsp->Pri.Learn_E.Result.Sta              =              MotAsp->In.NvM.E.Sta; 
     //      MotAsp->Pri.Learn_E.M_Result.Val            =              MotAsp->In.NvM.M.Val;
     //      MotAsp->Pri.Learn_E.M_Result.Sta            =              MotAsp->In.NvM.M.Sta;


     //      MotAsp->Pri.Learn_P.Result.Val              =              MotAsp->In.NvM.P.Val;
     //      MotAsp->Pri.Learn_P.Result.Sta              =              MotAsp->In.NvM.P.Sta; 

     //      MotAsp->Pri.P_Abs.Last_Rst_Pos              =              MotAsp->In.NvM.Rst_P;

     //      MotAsp->Pri.P_Rel.Last_Rst_Pos              =              MotAsp->In.NvM.Rst_P;   
     // }

}


/*
*Function :                   MotAsp_Mgr_Out                                 
*Brief    :         Output function
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                        
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Out(MotAsp_Mgr_ObjType *MotAsp)
{
     // MotAsp->Out.Ctrl.PosRel_01mm            =              MotAsp->Pri.P_Rel.Result;
     // MotAsp->Out.Ctrl.PosRel_Sta             =              MotAsp->Pri.Learn_E.Result.Sta;

     // MotAsp->Out.Ctrl.PosAbs_01mm            =              MotAsp->Pri.P_Abs.Result;
     // MotAsp->Out.Ctrl.PosAbs_Sta             =              MotAsp->Pri.Learn_P.Result.Sta;

     // MotAsp->Out.Ctrl.SpdRpm                 =              MotAsp->Pri.Spd_Calc.Spd;

     // if(MotAsp_Base_SMode_OpenLoop == MotAsp->In.Ctrl.Mode)
     // {
     //      MotAsp->Out.Drv.eAngle             =              MotAsp->Pri.Open.Angle;
     // }
     // else
     // {
     //      MotAsp->Out.Drv.eAngle             =              MotAsp->Pri.MPS.Arctan.Angle - MotAsp->Pri.Learn_E.Result.Val;
     //      MotAsp->Out.Drv.eAngleSta          =              MotAsp->Pri.Learn_E.Result.Sta;

     //      MotAsp->Out.Ctrl.mAngle            =              MotAsp->Pri.MPS.Sp.M_Angle - MotAsp->Pri.Learn_E.M_Result.Val;
     //      MotAsp->Out.Ctrl.mAngleSta         =              MotAsp->Pri.Learn_E.M_Result.Sta;
     // }


     
     // if(MotAsp_Base_Valid == MotAsp->Pri.Learn_E.Result.Sta)
     // {
     //      MotAsp->Out.NvM.E.Val              =              MotAsp->Pri.Learn_E.Result.Val;
     //      MotAsp->Out.NvM.E.Sta              =              MotAsp->Pri.Learn_E.Result.Sta;   
     // }

     // if(MotAsp_Base_Valid == MotAsp->Pri.Learn_E.M_Result.Sta)
     // {
     //      MotAsp->Out.NvM.M.Val              =              MotAsp->Pri.Learn_E.M_Result.Val;
     //      MotAsp->Out.NvM.M.Sta              =              MotAsp->Pri.Learn_E.M_Result.Sta;   
     // }


     // if(MotAsp_Base_Valid == MotAsp->Pri.Learn_P.Result.Sta)
     // {
     //      MotAsp->Out.NvM.P.Val              =              MotAsp->Pri.Learn_P.Result.Val;
     //      MotAsp->Out.NvM.P.Sta              =              MotAsp->Pri.Learn_P.Result.Sta;   
     // }

     // MotAsp->Out.NvM.Rst_P                   =              MotAsp->Pri.P_Abs.Pos - MotAsp->Pri.P_Abs.Pos_Basic + MotAsp->Pri.P_Abs.Last_Rst_Pos;
}

/*
*Function :                   MotAsp_Mgr_Diag                                 
*Brief    :         Diagnostic processing functions 
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                           
*Return   :                                   NULL                                             
 */
uint8_T MotAsp_Mgr_Diag(MotAsp_Mgr_ObjType *MotAsp)
{
     uint8_T res = 0u;
     uint8_T i = 0u;

     for(i = 0u; i < MotAsp_Lib_SIN_COS_SIG_NUM ; i++)
     {
          if(MotAsp_Base_Valid == MotAsp_Diag_SiCo_i_Fault(&(MotAsp->Pri.MPS.D_SiCo_i[i])))
          {
               res |= 0x01U << i;
          }
     }

     if(MotAsp_Base_Valid == MotAsp_Diag_SiCo_Fault(&(MotAsp->Pri.MPS.D_sico)))
     {
          res = (0x01U << MotAsp_Lib_SIN_COS_SIG_NUM) - 0x01U;
     }

     return res;
}

/*
*Function :                   MotAsp_Mgr_State_Date_Trans                                 
*Brief    :         State information conversion function
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                            
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_State_Date_Trans(MotAsp_Mgr_ObjType *MotAsp)
{
     MotAsp_Lib_Mps_Handler(&(MotAsp->Pri.MPS));
     MotAsp->Out.Ctrl.Sta = MotAsp_Base_AspMcs_Noraml;

     switch(MotAsp->In.Ctrl.Mode)
     {
          case MotAsp_Base_SMode_Un:

          break;
          case MotAsp_Base_SMode_ForceAlignment:
               MotAsp->Pri.Learn_E.M_Angle = MotAsp->Pri.MPS.Sp.M_Angle;
               MotAsp->Pri.Learn_E.Angle = MotAsp->Pri.MPS.Arctan.Angle;
               MotAsp_Mgr_Learn_E_Zero(&(MotAsp->Pri.Learn_E));
               MotAsp->Out.Ctrl.Sta = MotAsp_Base_AspMcs_CalAngle;
          break;
          case MotAsp_Base_SMode_Record_PosZero:
               MotAsp->Pri.Learn_P.Pos = MotAsp->Pri.MPS.Sp.Angle;
               MotAsp_Mgr_Learn_P_Zero(&(MotAsp->Pri.Learn_P));
               MotAsp->Out.Ctrl.Sta = MotAsp_Base_AspMcs_CalPos;
          break;
          case MotAsp_Base_SMode_OpenLoop:
               MotAsp_Mgr_Vtaul_Calc(&(MotAsp->Pri.Open));
          break;
          case MotAsp_Base_SMode_CLTorque:

          break;
          case MotAsp_Base_SMode_CLSpeed:

          break;
          case MotAsp_Base_SMode_CLPos:

          break;
          default:

               break;
     }
     MotAsp->Pri.Spd_Simp.Spd = (uint16_T)MotAsp->Pri.MPS.Sp.spd;
     MotAsp_Mgr_Spd_Simple(&(MotAsp->Pri.Spd_Simp));

}


/*
*Function :                  MotAsp_Mgr_Init                                
*Brief    :         Initialize the information function
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                           
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Init(MotAsp_Mgr_ObjType *MotAsp)
{
     for(uint8_T i = 0U ; i < MotAsp_Lib_SIN_COS_SIG_NUM ; i++)
     {
          MotAsp->Pri.MPS.D_SiCo_i[i].V_Range = MotAsp_Cfg_SiCo_V_Range_Para[i]; 
          MotAsp->Pri.MPS.D_SiCo_i[i].SiCo_F = MotAsp_Cfg_Sin_Cos_i_Cnt_Para[i];
     }

     MotAsp->Pri.MPS.D_sico.Limit_D = MotAsp_Cfg_SiCo_Calib_Para;
     MotAsp->Pri.MPS.D_sico.SiCs_DF = MotAsp_Cfg_SiCo_Cnt_Para;

     for(uint8_T i = 0U ; i < MotAsp_Lib_SiCo_NUM ; i++)
     {
          MotAsp->Pri.MPS.F_Sico[i] = MotAsp_Cfg_Sin_Filter_Para[i];

          MotAsp->Pri.MPS.C_SiCo[i] = MotAsp_Cfg_Calc_MIA_Para[i];
          MotAsp->Pri.MPS.C_SiCo[i].max = MotAsp_Cfg_Pi_Para[MotAsp_Lib_SIN_0 + i * MotAsp_Lib_SiCo_NUM];
          MotAsp->Pri.MPS.C_SiCo[i].min = MotAsp_Cfg_Pi_Para[MotAsp_Lib_SIN_1 + i * MotAsp_Lib_SiCo_NUM];
          MotAsp->Pri.MPS.C_SiCo[i].MIA = MotAsp_Cfg_Calc_MIA_Algth_Para[i];
          MotAsp->Pri.MPS.C_SiCo[i].MIA.Updata_Max = MotAsp_Cfg_SiCo_MIN_Cnt_Para[MotAsp_Lib_SIN_0 + i * MotAsp_Lib_SiCo_NUM];
          MotAsp->Pri.MPS.C_SiCo[i].MIA.Updata_Min = MotAsp_Cfg_SiCo_MIN_Cnt_Para[MotAsp_Lib_SIN_1 + i * MotAsp_Lib_SiCo_NUM];
     }

     MotAsp->Pri.MPS.Arctan = MotAsp_Cfg_Calc_Arctan_Para;
     MotAsp->Pri.MPS.Arctan.Basic = MotAsp_Cfg_Quadrant_Basic;

     MotAsp->Pri.MPS.Sp = MotAsp_Cfg_Calc_Angle_Para;

     MotAsp->Pri.Open = MotAsp_Cfg_Virtual;

     MotAsp->Pri.Spd_Simp = MotAsp_Cfg_Spd_Simp;

     MotAsp->Pri.Spd_Calc = MotAsp_Cfg_Spd_Calc;

     MotAsp->Pri.P_Abs = MotAsp_Cfg_Abs_Pos;

     MotAsp->Pri.P_Rel = MotAsp_Cfg_Rel_Pos;
}


/*
*Function :                   MotAsp_Mgr_Angle_Handler                                 
*Brief    :         Angle information solving
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                            
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Angle_Handler(MotAsp_Mgr_ObjType *MotAsp)
{
     switch(MotAsp->Sta)
     {
          case MotAsp_Idle:
          break;
          case MotAsp_Start:
               // if(MotAsp->In.NvM.En)
               // {
                    MotAsp_Mgr_In(MotAsp);

                    MotAsp_Mgr_State_Date_Trans(MotAsp);

                    MotAsp->Out.Ctrl.ErrCode = MotAsp_Mgr_Diag(MotAsp);

                    MotAsp_Mgr_Out(MotAsp);
               // }
               // else
               // {

               // } 
          break;
          default:
          break;
     }
     
}

/*
*Function :                   MotAsp_Mgr_Spd_Pos_Handler                                 
*Brief    :         Velocity and position information solving
*Para     :         MotAsp_Mgr_ObjType *MotAsp: 
                         typedef struct
                         {
                         const MotAsp_Mgr_BspType * const P_Bsp;
                         const MotAsp_Mgr_SetType * const P_Set;
                         MotAsp_Mgr_InType    In;
                         MotAsp_Mgr_DebugType Debug;
                         MotAsp_Mgr_OutType   Out;
                         MotAsp_Mgr_PriType   Pri;
                         } MotAsp_Mgr_ObjType;                            
*Return   :                                   NULL                                             
 */
void MotAsp_Mgr_Spd_Pos_Handler(MotAsp_Mgr_ObjType *MotAsp)
{
     switch(MotAsp->Sta)
     {
         case MotAsp_Idle:
               MotAsp_Mgr_Init(MotAsp);
         break;
         case MotAsp_Start:
          //     if(MotAsp->In.NvM.En)
          //      {
                    MotAsp_Mgr_Spd_Calc(&(MotAsp->Pri.Spd_Calc));

                    MotAsp->Pri.P_Abs.Pos = MotAsp->Pri.MPS.Sp.Angle;

                    MotAsp->Pri.P_Abs.P_Learn = MotAsp->Pri.Learn_P.Result.Val;

                    if((MotAsp_Base_Valid == MotAsp->Pri.Learn_P.State)&&(MotAsp_Base_Valid == MotAsp->Pri.Learn_P.En))
                    {
                         MotAsp->Pri.Learn_E.State = MotAsp_Base_InValid;
                         MotAsp->Pri.P_Abs.State = MotAsp_Type_ABS_POS_CALC_LEARN_P;
                    }

                    MotAsp_Mgr_Pos_Calc(&(MotAsp->Pri.P_Abs));

                    MotAsp->Pri.P_Rel.Pos = MotAsp->Pri.MPS.Sp.Angle;

                    MotAsp->Pri.P_Rel.P_Learn = MotAsp->Pri.Learn_P.Result.Val;

                    if(((MotAsp_Base_Valid == MotAsp->Pri.Learn_E.State)&&(MotAsp_Base_Valid == MotAsp->Pri.Learn_E.En))||\
                         ((MotAsp_Base_Valid == MotAsp->Pri.Learn_P.State)&&(MotAsp_Base_Valid == MotAsp->Pri.Learn_P.En))
                    )
                    {
                         MotAsp->Pri.Learn_E.State = MotAsp_Base_InValid;
                         MotAsp->Pri.P_Rel.State = MotAsp_Type_ABS_POS_CALC_LEARN_P;
                    }
                    MotAsp_Mgr_Pos_Calc(&(MotAsp->Pri.P_Rel));
               // }
               // else
               // {

               // }
         break;
         default:
         break;
     }
}














